package kit.edu.lego.gruppe3.levels.door;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class MoveToRightWall implements Behavior{

	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();

	private int minDistance = 18;
	private int maxDistance = 21;
	
	private boolean distanceToClose = false;
	private boolean distanceToFar = false;
	
	private int distance;

	@Override
	public void action() {
		LCD.drawString("FRW", 0,1);
		if (distanceToClose){
			robot.setTravelSpeed(5);
			robot.steer(60);
			
		}
		
		if (distanceToFar){
			robot.setTravelSpeed(5);
			robot.steer(-60);
		}

	}

	@Override
	public void suppress() {
		
		
	}

	@Override
	public boolean takeControl() {
		distance = ComponentsRepository.getInstance().getSonicSensor().getDistance();
		distanceToClose = minDistance > distance;
		distanceToFar = maxDistance < distance;
		return LevelSelector.getInstance().isDore() &&
			 (distanceToClose || distanceToFar);
	}

}